#include <iostream> // 输入输出流库
#include <pcl/point_cloud.h> // 点云头文件
#include <pcl/point_types.h> // 点类型头文件
#include <pcl/io/pcd_io.h> // PCD文件读写头文件
#include <pcl/visualization/pcl_visualizer.h> // 点云可视化头文件
#include <pcl/registration/icp.h> // ICP算法头文件
#include <Eigen/Dense> // 线性代数库

#include "optimized_ICP_GN.h" // 自定义的优化ICP算法头文件

using namespace std; // 使用标准命名空间

int main() // 主函数
{
    // 创建点云对象的智能指针
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_opti_transformed_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_svd_transformed_ptr(new pcl::PointCloud<pcl::PointXYZ>);

    // 读取点云数据文件
    pcl::io::loadPCDFile("../data/room_scan2.pcd", *cloud_target_ptr); // 目标点云
    pcl::io::loadPCDFile("../data/room_scan1.pcd", *cloud_source_ptr); // 源点云

    // 创建变换矩阵并设置为单位矩阵
    Eigen::Matrix4f T_predict, T_final;
    T_predict.setIdentity(); // 设置为单位矩阵
    // 设置预测的变换矩阵，用于ICP算法的初始估计
    T_predict << 0.765, 0.643, -0.027, -1.472,
        -0.6, 0.765, -0.023, 1.3,
        0.006, 0.035, 0.999, -0.1,
        0, 0, 0, 1;

    std::cout << "Wait, matching..." << std::endl; // 输出匹配信息

    // =======================   optimized icp   =======================
    // 创建优化ICP对象
    OptimizedICPGN optimized_icp_gn;
    // 设置目标点云和算法参数
    optimized_icp_gn.SetTargetCloud(cloud_target_ptr);
    optimized_icp_gn.SetMaxIterations(30); // 最大迭代次数
    optimized_icp_gn.SetMaxCorrespondDistance(0.3); // 最大对应距离
    optimized_icp_gn.SetTransformationEpsilon(1e-4); // 变换矩阵的收敛阈值
    // 执行优化ICP算法
    optimized_icp_gn.Match(cloud_source_ptr, T_predict, cloud_source_opti_transformed_ptr, T_final);
    std::cout << "\n============== Optimized ICP =================" << std::endl;
    std::cout << "T final: \n"
              << T_final << std::endl; // 输出最终变换矩阵
    std::cout << "fitness score: " << optimized_icp_gn.GetFitnessScore() << std::endl; // 输出拟合分数

    // =======================   optimized icp   =======================

    // =======================   svd icp   =======================
    // 创建SVD ICP对象
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp_svd;
    icp_svd.setInputTarget(cloud_target_ptr); // 设置目标点云
    icp_svd.setInputSource(cloud_source_ptr); // 设置源点云
    icp_svd.setMaxCorrespondenceDistance(0.3); // 设置最大对应距离
    icp_svd.setMaximumIterations(30); // 设置最大迭代次数
    icp_svd.setEuclideanFitnessEpsilon(1e-4); // 设置欧几里得距离的收敛阈值
    icp_svd.setTransformationEpsilon(1e-4); // 设置变换矩阵的收敛阈值
    // 执行SVD ICP算法
    icp_svd.align(*cloud_source_svd_transformed_ptr, T_predict);
    std::cout << "\n============== SVD ICP =================" << std::endl;
    std::cout << "T final: \n"
              << icp_svd.getFinalTransformation() << std::endl; // 输出最终变换矩阵
    std::cout << "fitness score: " << icp_svd.getFitnessScore() << std::endl; // 输出拟合分数
    // =======================   svd icp   =======================

    // 可视化
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("viewer"));
    viewer->initCameraParameters(); // 初始化相机参数

    int v1(0); // 创建视窗1
    int v2(1);
    viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1); // 设置视窗1的位置和大小
    viewer->setBackgroundColor(0, 0, 0, v1); // 设置视窗1的背景颜色
    viewer->addText("Optimized ICP", 10, 10, "optimized icp", v1); // 在视窗1中添加文本
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_opti_color(cloud_source_opti_transformed_ptr,255, 0,0); // 设置点云颜色为红色
    viewer->addPointCloud<pcl::PointXYZ>(cloud_source_opti_transformed_ptr, source_opti_color, "source opti cloud", v1); // 在视窗1中添加优化ICP后的点云
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color_0(cloud_target_ptr, 0, 0, 255); // 设置点云颜色为蓝色
    viewer->addPointCloud<pcl::PointXYZ>(cloud_target_ptr, target_color_0, "target cloud1", v1); // 在视窗1中添加目标点云

    viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2); // 创建视窗2
    viewer->setBackgroundColor(0.0, 0.0, 0.0, v2); // 设置视窗2的背景颜色
    viewer->addText("SVD ICP", 10, 10, "svd icp", v2); // 在视窗2中添加文本
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color_1(cloud_target_ptr, 0, 0, 255); // 设置点云颜色为蓝色
    viewer->addPointCloud<pcl::PointXYZ>(cloud_target_ptr, target_color_1, "target cloud2", v2); // 在视窗2中添加目标点云
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_svd_color(cloud_source_svd_transformed_ptr, 0, 255, 0); // 设置点云颜色为绿色
    viewer->addPointCloud<pcl::PointXYZ>(cloud_source_svd_transformed_ptr, source_svd_color, "source svd cloud", v2); // 在视窗2中添加SVD ICP后的点云

    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "source opti cloud"); // 设置点云的点大小
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "source svd cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "target cloud1");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "target cloud2");
    viewer->addCoordinateSystem(1.0); // 添加坐标系

    viewer->setCameraPosition(0, 0, 20, 0, 10, 10, v1); // 设置视窗1的相机位置
    viewer->setCameraPosition(0, 0, 20, 0, 10, 10, v2); // 设置视窗2的相机位置

    viewer->spin(); // 开始可视化循环

    return 0; // 返回0，程序结束
}